#include "VoxelGridFilter.h"
#include "dialogs/VoxelGridDialog.h"

#include <cc2sm.h>
#include <filtering.h>
#include <my_point_types.h>
#include <sm2cc.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>

VoxelGridFilter::VoxelGridFilter()
	: BaseFilter(FilterDescription("Voxel Grid Filter", "Downsamples the data", "Assembles a local 3D grid over a given pointcloud and downsamples + filters the data.", ":/toolbar/PclUtils/icons/voxel_grid.png", true))
	, m_dialog(0)
	, leafSizeX(0)
	, leafSizeY(0)
	, leafSizeZ(0)
	, downSampleAllData(false)
{

}

VoxelGridFilter::~VoxelGridFilter()
{
	if (m_dialog)
		delete m_dialog;
}

int VoxelGridFilter::openDialog()
{
	if (!m_dialog)
	{
		m_dialog = new VoxelGridDialog;
	}

	ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
	//if (cloud)
	//{
	//	ccBBox bBox = cloud->getBB(true,false);
	//	if (bBox.isValid())
	//		m_dialog->radiusDoubleSpinBox->setValue(bBox.getDiagNorm() * 0.005);
	//}

	return m_dialog->exec() ? 1 : 0;
}

void VoxelGridFilter::getParametersFromDialog()
{
	assert(m_dialog);
	if (!m_dialog)
		return;

    //fill in parameters from dialog
   
    leafSizeX = m_dialog->leafSizeXSpinBox->value();
    leafSizeY = m_dialog->leafSizeYSpinBox->value();
	leafSizeZ = m_dialog->leafSizeZSpinBox->value();
	
	downSampleAllData = m_dialog->downsampleAllCheckBox->checkState();
	
}

int VoxelGridFilter::compute()
{

	ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
	if(!cloud)
		return -1;

	sensor_msgs::PointCloud2::Ptr sm_cloud (new sensor_msgs::PointCloud2);

	//get pointcloud in sensor_msgs format
    cc2smReader converter;
    converter.setInputCloud(cloud);
	converter.getAsSM(*sm_cloud);

	pcl::VoxelGrid<sensor_msgs::PointCloud2> voxelgrid;
	voxelgrid.setInputCloud(sm_cloud);
	voxelgrid.setLeafSize(leafSizeX,leafSizeY,leafSizeZ);
	voxelgrid.setDownsampleAllData(downSampleAllData);

	sensor_msgs::PointCloud2::Ptr out_sm_cloud (new sensor_msgs::PointCloud2);

	voxelgrid.filter(*out_sm_cloud);

	boost::shared_ptr<sensor_msgs::PointCloud2> out_cloud_ptr = boost::make_shared<sensor_msgs::PointCloud2>(*out_sm_cloud);

	sm2ccReader reader;
	ccPointCloud* out_c = new ccPointCloud;
	reader.setInputCloud(out_cloud_ptr);
	int good = reader.getAsCC(out_c);
	if(good!=1){
		delete out_c;
		return -1;
	}

	ccHObject* cloud_container = new ccHObject();

	cloud_container->addChild(out_c);
	cloud_container->setName(qPrintable("voxelgrid"));

	emit newEntity(cloud_container);

	
	return 1;
}
